﻿using System;
using System.Collections;
using System.Collections.Generic;

// Algorithms taken from Press,WH et al. (Eds.) Numerical Recipes in C:  The Art of Scientific Computing.  Cambridge, MA:  Cambridge.
namespace NumericalRecipes
{
	/// <summary>
	/// Cholesky factorization of a matrix (taken from Numerical Recipes).
	/// </summary>
	public class CholeskyFactorizor
	{
		private int i,j,k,n;
		/// <summary>
		/// Computes the Cholesky factor of a symmetric, positive semidefinite matrix U.
		/// </summary>
		/// <param name="U">input, The matrix to be factored.  Only the upper-right triangle is used (including the diagonal).</param>
		/// <param name="output">output, The Cholesky factor as a lower-left triangular matrix.  Elements above the diagonal are meaningless.</param>
		/// <returns>False if the matrix was not positive semidefinite, meaning that a Cholesky factor could not be computed.  True otherwise.</returns>
		public bool Run(double[,] U,double[,] output)
		{
			double sum;
			n = U.GetLength(0);
			for (i=0;i<n;i++)
			{
				for (j=i;j<n;j++)
				{
					sum=U[i,j];
					for (k=i-1;k>=0;k--)
						sum -= U[i,k]*U[j,k];
					if (i == j)
					{
						if (sum <= 0.0)
							return false;
						output[i,i]=Math.Sqrt(sum);
					}
					else
						output[j,i]=sum/output[i,i];
				}
			}
			return true;
		}
		/// <summary>
		/// Computes the Cholesky factor of a symmetric, positive semidefinite matrix U.
		/// </summary>
		/// <param name="U">input, The matrix to be factored.  Only the upper-right triangle is used (including the diagonal).</param>
		/// <param name="output">output, The Cholesky factor as a lower-left triangular matrix.  Elements above the diagonal are meaningless.</param>
		/// <returns>False if the matrix was not positive semidefinite, meaning that a Cholesky factor could not be computed.  True otherwise.</returns>
		public bool Run(float[,] U, float[,] output)
		{
			float sum;
			n = U.GetLength(0);
			for (i=0;i<n;i++)
			{
				for (j=i;j<n;j++)
				{
					sum=U[i,j];
					for (k=i-1;k>=0;k--)
						sum -= U[i,k]*U[j,k];
					if (i == j)
					{
						if (sum <= 0.0f)
							return false;
						output[i,i]=(float)Math.Sqrt((double)sum);
					}
					else
						output[j,i]=sum/output[i,i];
				}
			}
			return true;
		}
		/// <summary>
		/// Computes the Cholesky factor of a symmetric, positive semidefinite matrix U.
		/// </summary>
		/// <param name="U">input, The matrix to be factored.  Only the upper-right triangle is used (including the diagonal).</param>
		/// <param name="output">output, The Cholesky factor as a lower-left triangular matrix.  Elements above the diagonal are meaningless.</param>
		/// <returns>False if the matrix was not positive semidefinite, meaning that a Cholesky factor could not be computed.  True otherwise.</returns>
		public bool Run(decimal[,] U,decimal[,] output)
		{
			decimal sum;
			n = U.GetLength(0);
			for (i=0;i<n;i++)
			{
				for (j=i;j<n;j++)
				{
					sum=U[i,j];
					for (k=i-1;k>=0;k--)
						sum -= U[i,k]*U[j,k];
					if (i == j)
					{
						if (sum <= 0.0m)
							return false;
						output[i,i]=(decimal)Math.Sqrt((double)sum);
					}
					else
						output[j,i]=sum/output[i,i];
				}
			}
			return true;
		}
	}
	/// <summary>
	/// Matrix inversion by LU Decomposition and then Backsubstitution (taken from Numereical Recipes).
	/// </summary>
	public class MatrixInvertor
	{
		private struct MemFloat
		{
			public float d;
			public float[,] a;
			public float[] col,vv;
			public float TINY;
		}
		private struct MemDouble
		{
			public double d;
			public double[,] a;
			public double[] col,vv;
			public double TINY;
		}
		private struct MemDecimal
		{
			public decimal d;
			public decimal[,] a;
			public decimal[] col,vv;
			public decimal TINY;
		}
		private int[] indx,b;
		private MemFloat mf = new MemFloat();
		private MemDouble md = new MemDouble();
		private MemDecimal mm = new MemDecimal();
		public MatrixInvertor()
		{
			mf.TINY=(float)(1.0e-18);
			md.TINY=(1.0e-36);
			mm.TINY=(decimal)(1.0e-72);
		}
		/// <summary>
		/// Computes the inverse of a matrix using LU decomposition and backsubstitution.
		/// </summary>
		/// <param name="A">The matrix to be evaluated for its inverse.  A is not altered.</param>
		/// <param name="output">The output (the matrix inverse of A).</param>
		/// <returns>Returns true if successful, false otherwise.  If false, matrix A is probably singular.</returns>
		public bool Run(double[,] A,double[,] output)
		{
			int i,j;
			int len = A.GetLength(0);

			//------------------------------------------------------
			//	Memory Allocation
			//------------------------------------------------------
			if (md.a==null || md.a.GetLength(0)<len)
				md.a = new double[len,len];

			if (indx==null || indx.Length < len)
				indx = new int[len];

			if (b==null || b.Length < len)
				b = new int[len];

			if (md.col==null || md.col.Length < len)
				md.col = new double[len];
			//------------------------------------------------------

			//	Initialize
			for (i=0;i<len;i++)
				for (j=0;j<len;j++)
					md.a[i,j]=A[i,j];
			md.d = 1.0;

			//	Inversion by LU decompusition and then back-substitution
			if (!Ludcmp(md.a,len,indx,ref md.d))
				return false;
			else
			{
				for (j=0;j < len;j++)
				{
					for (i=0;i < len;i++)
						md.col[i] = 0.0;
					md.col[j] = 1.0;
					Lubksb(md.a,len,indx,md.col);
					for (i=0;i < len;i++)
						output[i,j] = md.col[i];
				}
			}
			return true;
		}
		/// <summary>
		/// Computes the inverse of a matrix using LU decomposition and backsubstitution.
		/// </summary>
		/// <param name="A">The matrix to be evaluated for its inverse.  A is not altered.</param>
		/// <param name="output">The output (the matrix inverse of A).</param>
		/// <returns>Returns true if successful, false otherwise.  If false, matrix A is probably singular.</returns>
		public bool Run(float[,] A,float[,] output)
		{
			int i,j;
			int len = A.GetLength(0);

			//------------------------------------------------------
			//	Memory Allocation
			//------------------------------------------------------
			if (mf.a==null || mf.a.GetLength(0)<len)
				mf.a = new float[len,len];

			if (indx==null || indx.Length < len)
				indx = new int[len];

			if (b==null || b.Length < len)
				b = new int[len];

			if (mf.col==null || mf.col.Length < len)
				mf.col = new float[len];
			//------------------------------------------------------

			//	Initialize
			for (i=0;i<len;i++)
				for (j=0;j<len;j++)
					mf.a[i,j]=A[i,j];
			mf.d = 1.0f;

			//	Inversion by LU decompusition and then back-substitution
			if (!Ludcmp(mf.a,len,indx,ref mf.d))
				return false;
			else
			{
				for (j=0;j < len;j++)
				{
					for (i=0;i < len;i++)
						mf.col[i] = 0.0f;
					mf.col[j] = 1.0f;
					Lubksb(mf.a,len,indx,mf.col);
					for (i=0;i < len;i++)
						output[i,j] = mf.col[i];
				}
			}
			return true;
		}
		/// <summary>
		/// Computes the inverse of a matrix using LU decomposition and backsubstitution.
		/// </summary>
		/// <param name="A">The matrix to be evaluated for its inverse.  A is not altered.</param>
		/// <param name="output">The output (the matrix inverse of A).</param>
		/// <returns>Returns true if successful, false otherwise.  If false, matrix A is probably singular.</returns>
		public bool Run(decimal[,] A,decimal[,] output)
		{
			int i,j;
			int len = A.GetLength(0);

			//------------------------------------------------------
			//	Memory Allocation
			//------------------------------------------------------
			if (mm.a==null || mm.a.GetLength(0)<len)
				mm.a = new decimal[len,len];

			if (indx==null || indx.Length < len)
				indx = new int[len];

			if (b==null || b.Length < len)
				b = new int[len];

			if (mm.col==null || mm.col.Length < len)
				mm.col = new decimal[len];
			//------------------------------------------------------

			//	Initialize
			for (i=0;i<len;i++)
				for (j=0;j<len;j++)
					mm.a[i,j]=A[i,j];
			mm.d = 1.0m;

			//	Inversion by LU decompusition and then back-substitution
			if (!Ludcmp(mm.a,len,indx,ref mm.d))
				return false;
			else
			{
				for (j=0;j < len;j++)
				{
					for (i=0;i < len;i++)
						mm.col[i] = 0.0m;
					mm.col[j] = 1.0m;
					Lubksb(mm.a,len,indx,mm.col);
					for (i=0;i < len;i++)
						output[i,j] = mm.col[i];
				}
			}
			return true;
		}
		/// <summary>
		/// Calculates a LU Decomposition of a column permutation of a.
		/// </summary>
		/// <param name="a">The input matrix for LU Decomp.  Input is destroyed and outputs the LU Decomp of a column permutation of a.</param>
		/// <param name="n">The length of a side of the square matrix, 'a'.</param>
		/// <param name="indx">This is an output vector that records the row permutations created by partial pivoting.</param>
		/// <param name="d">This is an output value that equals + or - 1.0 depending on whether or the number of row interchanges was even or odd respectively. </param>
		/// <returns>False if a is singular, true otherwise.</returns>
		private bool Ludcmp(double[,] a,int n,int[] indx,ref double d)
		{
			int i,j,k;
			double big,dum,sum,temp;
			int imax = 0;

			if (md.vv==null || md.vv.Length<n)
				md.vv = new double[n];

			d=1.0;
			for (i=0;i<n;i++)
			{
				big=0.0;
				for (j=0;j<n;j++)
					if ((temp=Math.Abs(a[i,j])) > big) big=temp;
				if (big == 0.0)
					return false;
				md.vv[i]=1.0/big;
			}
			for (j=0;j<n;j++)
			{
				for (i=0;i<j;i++)
				{
					sum=a[i,j];
					for (k=0;k<i;k++)
						sum -= a[i,k]*a[k,j];
					a[i,j]=sum;
				}
				big=0.0;
				for (i=j;i<n;i++)
				{
					sum=a[i,j];
					for (k=0;k<j;k++)
						sum -= a[i,k]*a[k,j];
					a[i,j]=sum;
					if ((dum=md.vv[i]*Math.Abs(sum)) >= big)
					{
						big=dum;
						imax=i;
					}
				}
				if (j != imax)
				{
					for (k=0;k<n;k++)
					{
						dum=a[imax,k];
						a[imax,k]=a[j,k];
						a[j,k]=dum;
					}
					d = -d;
					md.vv[imax]=md.vv[j];
				}
				indx[j]=imax;
				if (a[j,j] == 0.0) a[j,j]=md.TINY;
				if (j != n-1)
				{
					dum=1.0/(a[j,j]);
					for (i=j+1;i<n;i++)
						a[i,j] *= dum;
				}
			}
			return true;
		}
		/// <summary>
		/// Calculates a LU Decomposition of a column permutation of a.
		/// </summary>
		/// <param name="a">The input matrix for LU Decomp.  Input is destroyed and outputs the LU Decomp of a column permutation of a.</param>
		/// <param name="n">The length of a side of the square matrix, 'a'.</param>
		/// <param name="indx">This is an output vector that records the row permutations created by partial pivoting.</param>
		/// <param name="d">This is an output value that equals + or - 1.0 depending on whether or the number of row interchanges was even or odd respectively. </param>
		/// <returns>False if a is singular, true otherwise.</returns>
		private bool Ludcmp(float[,] a,int n,int[] indx,ref float d)
		{
			int i,j,k;
			float big,dum,sum,temp;
			int imax = 0;

			if (mf.vv==null || mf.vv.Length<n)
				mf.vv = new float[n];

			d=1.0f;
			for (i=0;i<n;i++)
			{
				big=0.0f;
				for (j=0;j<n;j++)
					if ((temp=Math.Abs(a[i,j])) > big) big=temp;
				if (big == 0.0)
					return false;
				mf.vv[i]=1.0f/big;
			}
			for (j=0;j<n;j++)
			{
				for (i=0;i<j;i++)
				{
					sum=a[i,j];
					for (k=0;k<i;k++)
						sum -= a[i,k]*a[k,j];
					a[i,j]=sum;
				}
				big=0.0f;
				for (i=j;i<n;i++)
				{
					sum=a[i,j];
					for (k=0;k<j;k++)
						sum -= a[i,k]*a[k,j];
					a[i,j]=sum;
					if ((dum=mf.vv[i]*Math.Abs(sum)) >= big)
					{
						big=dum;
						imax=i;
					}
				}
				if (j != imax)
				{
					for (k=0;k<n;k++)
					{
						dum=a[imax,k];
						a[imax,k]=a[j,k];
						a[j,k]=dum;
					}
					d = -d;
					mf.vv[imax]=mf.vv[j];
				}
				indx[j]=imax;
				if (a[j,j] == 0.0) a[j,j]=mf.TINY;
				if (j != n-1)
				{
					dum=1.0f/(a[j,j]);
					for (i=j+1;i<n;i++)
						a[i,j] *= dum;
				}
			}
			return true;
		}
		/// <summary>
		/// Calculates a LU Decomposition of a column permutation of a.
		/// </summary>
		/// <param name="a">The input matrix for LU Decomp.  Input is destroyed and outputs the LU Decomp of a column permutation of a.</param>
		/// <param name="n">The length of a side of the square matrix, 'a'.</param>
		/// <param name="indx">This is an output vector that records the row permutations created by partial pivoting.</param>
		/// <param name="d">This is an output value that equals + or - 1.0 depending on whether or the number of row interchanges was even or odd respectively. </param>
		/// <returns>False if a is singular, true otherwise.</returns>
		private bool Ludcmp(decimal[,] a,int n,int[] indx,ref decimal d)
		{
			int i,j,k;
			decimal big,dum,sum,temp;
			int imax = 0;

			if (mm.vv==null || mm.vv.Length<n)
				mm.vv = new decimal[n];

			d=1.0m;
			for (i=0;i<n;i++)
			{
				big=0.0m;
				for (j=0;j<n;j++)
					if ((temp=Math.Abs(a[i,j])) > big) big=temp;
				if (big == 0.0m)
					return false;
				mm.vv[i]=1.0m/big;
			}
			for (j=0;j<n;j++)
			{
				for (i=0;i<j;i++)
				{
					sum=a[i,j];
					for (k=0;k<i;k++)
						sum -= a[i,k]*a[k,j];
					a[i,j]=sum;
				}
				big=0.0m;
				for (i=j;i<n;i++)
				{
					sum=a[i,j];
					for (k=0;k<j;k++)
						sum -= a[i,k]*a[k,j];
					a[i,j]=sum;
					if ((dum=mm.vv[i]*Math.Abs(sum)) >= big)
					{
						big=dum;
						imax=i;
					}
				}
				if (j != imax)
				{
					for (k=0;k<n;k++)
					{
						dum=a[imax,k];
						a[imax,k]=a[j,k];
						a[j,k]=dum;
					}
					d = -d;
					mm.vv[imax]=mm.vv[j];
				}
				indx[j]=imax;
				if (a[j,j] == 0.0m) a[j,j]=mm.TINY;
				if (j != n-1)
				{
					dum=1.0m/(a[j,j]);
					for (i=j+1;i<n;i++)
						a[i,j] *= dum;
				}
			}
			return true;
		}
		/// <summary>
		/// Uses LU Back-Substitution that solves the set of n linear equations Ax=b.
		/// </summary>
		/// <param name="a">Inputs the LU Decomposition of "A", determined by the function ludcmp.</param>
		/// <param name="n">The length of a side of the square matrix A.</param>
		/// <param name="indx">The index into the row permutations of the LU Decomposition.</param>
		/// <param name="b">The variable "b", also used for output, the solution to Ax=b</param>
		private void Lubksb(double[,] a,int n,int[] indx,double[] b)
		{
			int i,ii=0,ip,j;
			double sum;

			for (i=0;i<n;i++)
			{
				ip=indx[i];
				sum=b[ip];
				b[ip]=b[i];
				if (ii != 0)
					for (j=ii-1;j<i;j++)
						sum -= a[i,j]*b[j];
				else if (sum != 0.0)
					ii=i+1;
				b[i]=sum;
			}
			for (i=n-1;i>=0;i--)
			{
				sum=b[i];
				for (j=i+1;j<n;j++)
					sum -= a[i,j]*b[j];
				b[i]=sum/a[i,i];
			}
		}
		/// <summary>
		/// Uses LU Back-Substitution that solves the set of n linear equations Ax=b.
		/// </summary>
		/// <param name="a">Inputs the LU Decomposition of "A", determined by the function ludcmp.</param>
		/// <param name="n">The length of a side of the square matrix A.</param>
		/// <param name="indx">The index into the row permutations of the LU Decomposition.</param>
		/// <param name="b">The variable "b", also used for output, the solution to Ax=b</param>
		private void Lubksb(float[,] a,int n,int[] indx,float[] b)
		{
			int i,ii=0,ip,j;
			float sum;

			for (i=0;i<n;i++)
			{
				ip=indx[i];
				sum=b[ip];
				b[ip]=b[i];
				if (ii != 0)
					for (j=ii-1;j<i;j++)
						sum -= a[i,j]*b[j];
				else if (sum != 0.0f)
					ii=i+1;
				b[i]=sum;
			}
			for (i=n-1;i>=0;i--)
			{
				sum=b[i];
				for (j=i+1;j<n;j++)
					sum -= a[i,j]*b[j];
				b[i]=sum/a[i,i];
			}
		}
		/// <summary>
		/// Uses LU Back-Substitution that solves the set of n linear equations Ax=b.
		/// </summary>
		/// <param name="a">Inputs the LU Decomposition of "A", determined by the function ludcmp.</param>
		/// <param name="n">The length of a side of the square matrix A.</param>
		/// <param name="indx">The index into the row permutations of the LU Decomposition.</param>
		/// <param name="b">The variable "b", also used for output, the solution to Ax=b</param>
		private void Lubksb(decimal[,] a,int n,int[] indx,decimal[] b)
		{
			int i,ii=0,ip,j;
			decimal sum;

			for (i=0;i<n;i++)
			{
				ip=indx[i];
				sum=b[ip];
				b[ip]=b[i];
				if (ii != 0)
					for (j=ii-1;j<i;j++)
						sum -= a[i,j]*b[j];
				else if (sum != 0.0m)
					ii=i+1;
				b[i]=sum;
			}
			for (i=n-1;i>=0;i--)
			{
				sum=b[i];
				for (j=i+1;j<n;j++)
					sum -= a[i,j]*b[j];
				b[i]=sum/a[i,i];
			}
		}
	}
}